#ifndef MEAN_FILTER_HPP
#define MEAN_FILTER_HPP

#include <deque>
#include <rclcpp/rclcpp.hpp>
#include <builtin_interfaces/msg/time.hpp>

struct data_t
{
    double data;
    rclcpp::Time stamp;

    data_t(double data, rclcpp::Time stamp)
    {
        this->data = data;
        this->stamp = stamp;
    }
};


class MeanFilter
{
public:
    MeanFilter();
    MeanFilter(double filter_time)
    {
        this->filter_time = filter_time;
    }
    void add_data(double data);
    void update_covariance();
    double apply_filter(double input);
    double get_mean();
    double get_sum();
    void reset();
    double get_variance();
    void set_filter_time(double filter_time);
    double get_filter_time();

private:

    double filter_time = 10.0;
    double variance = 0;
    std::deque<data_t> data_list;

};

#endif // MEAN_FILTER_HPP
